/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "JPEGEncoder.hpp"
#include "TOccupancyGrid.h"
#include "glutImageGUI.h"
#include <unistd.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

void draw(TOccupancyGrid &O, unsigned char *rgb, int Nx, int Ny){
	O.fillRGB(rgb,1.0);
	displayImage(0, rgb,Ny,Nx);
	usleep(500*1000);
}

void testNormalDistribution(TOccupancyGrid &O, unsigned char *rgb, int Nx, int Ny){

	//grid_t empty=0.0;
	//grid_t rigid=254;
	
	Grid::TPixel pix;
	grid_t value;
	
	O.setGridToValue(0.1);
	
	float beamangle=55.0/180.0*M_PI;
	float r=0.20;///< sensor reading
	float a=10.5/180*M_PI; ///<sensor angle - half of the total beam angle
	O.setNormalDistributionArc(Grid::TCoord2D (0.06,0.035),beamangle,r,a);
	r=0.30;
	a=-27.5/180*M_PI;
	O.setNormalDistributionArc(Grid::TCoord2D (0.08,0.0),beamangle,r,a);
	r=0.45;
	a=-57.5/180*M_PI;
	O.setNormalDistributionArc(Grid::TCoord2D (0.06,-0.035),beamangle,r,a);
	
	for (int j=0;j<Ny;j++) {
		pix.y=j;
	  for (int i=0;i<Nx;i++) {
		pix.x=i;
		O.getValue(pix, value);
		value=value*255.0;
		O.setValue(pix, value);
		}	
	}	
	
	draw(O,rgb,Nx,Ny);
	O.save("occutestgrid.txt");
	
}

void testPrimitives(TOccupancyGrid &O, unsigned char *rgb, int Nx, int Ny){
	grid_t empty=0.0;
	grid_t rigid=254;
	grid_t occu=128;
	
	/// Set some primitives
	fprintf(stderr,"circle (0,0) radius 3\n");
	O.setCircle(Grid::TCoord2D(0,0),3.0, rigid);
	draw(O,rgb,Nx,Ny);
	fprintf(stderr,"circle (-5,0) radius 3\n");
	O.setCircle(Grid::TCoord2D(-5,0),3.0, rigid);
	draw(O,rgb,Nx,Ny);
		
	fprintf(stderr,"Line (0,0)->(5,5)\n");
	O.setLine(0,0,5,5,rigid);
	draw(O,rgb,Nx,Ny);
	
	fprintf(stderr,"Line (5,5)->(9,0)\n");
	O.setLine(5,5,9,0,rigid);
	draw(O,rgb,Nx,Ny);
	
	fprintf(stderr,"Line (9,0)->(-5-5)\n");
	O.setLine(9,0,-5,-5,rigid);
	draw(O,rgb,Nx,Ny);
	
    fprintf(stderr,"Add Line (0,9)->(-5-5)\n");
	O.addLine(0,9,-5,-5,rigid/3);
	draw(O,rgb,Nx,Ny);
    
    fprintf(stderr,"Add Line (0,9)->(-5-5)\n");
	O.addLine(0,9,-5,-5,rigid/3);
	draw(O,rgb,Nx,Ny);
    
	fprintf(stderr,"Beam (10,0,PI/2) width PI/8\n");
	O.setBeam(Grid::TCoord2D(10,0),10,M_PI/2,M_PI/8,occu);
	draw(O,rgb,Nx,Ny);
	
	fprintf(stderr,"Arc on top of the beam\n");
	O.setArc(Grid::TCoord2D(10,0),10+0.5,M_PI/2,M_PI/8,rigid);
	draw(O,rgb,Nx,Ny);
}

void testScan(TOccupancyGrid &O, unsigned char *rgb, int Nx, int Ny){
	Grid::TScan s;
	float a=0;
	s.alloc(180);
	grid_t empty=0.0;
	grid_t rigid=254;
	grid_t occu=128;
	fprintf(stderr,"1\n");
	for(int i=0;i<180;i++){
		s.r[i] = 5.0 + 3.0*sin(a*4);
		s.a[i] = a;
		a+=(M_PI/180.0); 
		//fprintf(stderr,"(%.2f %.2f) ",s.r[i],s.a[i] ); 
	}
	

	O.setScanPoints(-10, 0 ,0, s, occu);
	draw(O,rgb,Nx,Ny);
	
	O.setScanFillSimple(0, 0 ,0, s,empty,rigid);
	draw(O,rgb,Nx,Ny);
	
	fprintf(stderr,"Testing probabilistic way\n");
	usleep(1000*1000);
	O.setGridToValue(0.5);
	O.fillRGB(rgb,1.0/255.0);
	displayImage(0, rgb,Ny,Nx);
	usleep(500*1000);
	
	for(int i=0;i<5;i++){
		fprintf(stderr,"Adding a scan\n");
		O.setScanFillProb(0, 0 ,0, s,0.4,0.6);
		O.fillRGB(rgb,1.0/255.0);
		displayImage(0, rgb,Ny,Nx);
		usleep(500*1000);
	}
	fprintf(stderr,"Testing adding scan with rigid structures\n");
	O.setGridToValue(64);
	O.setLine(-5,0,-5,5,rigid);
	O.setLine(-5,5,5,5,rigid);
	O.setLine(5,5,5,0,rigid);
	draw(O,rgb,Nx,Ny);
	
	O.setScanFillWithRigidWalls(0, 
															0,
															0, 
															s, 
															empty,
															occu,
															rigid);

}
void testGetScan(TOccupancyGrid &O, unsigned char *rgb, int Nx, int Ny){
	Grid::TScan s;
	float a=0;
	s.alloc(180);
	grid_t empty=0.0;
	grid_t rigid=254;
	grid_t occu=128;
	
	O.setGridToValue(64);
	O.setLine(-5,0,-5,5,rigid);
	O.setLine(-5,5,5,5,rigid);
	O.setLine(5,5,5,0,rigid);
	draw(O,rgb,Nx,Ny);
	
	///Get scan from (0,0,0)
	O.getScan(0,0, 0, 180, M_PI/180, occu, s);
	///Draw it to other place
	O.setScanFillSimple(11, 0 ,0, s,empty,occu);
	
	draw(O,rgb,Nx,Ny);
}

void testMCL(TOccupancyGrid &O, unsigned char *rgb, int Nx, int Ny){
	Grid::TScan s;
	float a=0;
	s.alloc(180);
	grid_t empty=0.0;
	grid_t rigid=254;
	grid_t occu=128;
	long int s1,us1,s2,us2;
		
	O.setGridToValue(64);
	O.setLine(-15,0,-15,15,rigid);
	O.setLine(-15,15,15,15,rigid);
	O.setLine(15,15,15,0,rigid);
	draw(O,rgb,Nx,Ny);
	
	fprintf(stderr,"Starting efficiency test... might take a while");
	ownTime_GetTimeOfDay(&s1, 
											 &us1);

	for(float x = -5; x<5;x+=0.1){
		for(float y=0; y<5; y+=0.1){
			O.getScan(x,y, 0, 180, M_PI/180, occu, s);
		}
	}
	ownTime_GetTimeOfDay(&s2, 
												&us2);

	long int sec = s2-s1;
	long int us = us2-us1;
	
	long int ms = sec*1000+us/1000;
	
	fprintf(stderr,"It took %ldms to calculate 100*50*180=%ldms/line scanlines - happy?\n",
					ms,
				 ms/(100*50*180));
}

void testDistanceTransform(TOccupancyGrid &O, unsigned char *rgb, int Nx, int Ny){
	Grid::TScan s;
	s.alloc(180);
	
	grid_t empty=0.0;
	grid_t rigid=254;
	grid_t occu=128;
	long int s1,us1,s2,us2;
	float *dist=(float *) malloc(Nx*Ny*sizeof(float)) ;	
	float scale;
	
	O.setGridToValue(empty);
	O.setLine(-15,0,-15,15,rigid);
	O.setLine(-15,15,15,15,rigid);
	O.setLine(15,15,15,0,rigid);
	draw(O,rgb,Nx,Ny);
	
	fprintf(stderr,"Starting the transform... ");
	ownTime_GetTimeOfDay(&s1, 
												&us1);

	float maxd = O.distanceTransform(dist,occu);
	ownTime_GetTimeOfDay(&s2, 
												&us2);

	long int sec = s2-s1;
	long int us = us2-us1;
	
	long int ms = sec*1000+us/1000;
	
	fprintf(stderr,"It took %ldms to calculate\n",ms);
	scale = 255.0/maxd;
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			rgb[3*Ny*j+3*i] = (unsigned char) (dist[j*Ny+i]*scale);
			rgb[3*Ny*j+3*i+1] = (unsigned char) (dist[j*Ny+i]*scale);
			rgb[3*Ny*j+3*i+2] = (unsigned char) (dist[j*Ny+i]*scale);
		}
	}
	fprintf(stderr,"Max dist to obstacle seems to be %.3fm\n",maxd);
	displayImage(0, rgb,Ny,Nx);
	usleep(1000*1000);
	
	fprintf(stderr,"Starting efficiency test... might take a while");
	ownTime_GetTimeOfDay(&s1, 
												&us1);

	for(float x = -5; x<5;x+=0.1){
		for(float y=0; y<5; y+=0.1){
			O.getScan(x,y, 0, 180, M_PI/180, occu, dist, s);
		}
	}
	ownTime_GetTimeOfDay(&s2, 
												&us2);

	sec = s2-s1;
	us = us2-us1;
	
	ms = sec*1000+us/1000;
	
	fprintf(stderr,"It took %ldms to calculate 100*50*180=%.4fms/line scanlines - happy?\n",
					ms,
					(float)ms/(100.0*50.0*180.0));
	
	O.setGridToValue(64);
	O.setLine(-15,0,-15,15,rigid);
	O.setLine(-15,15,15,15,rigid);
	O.setLine(15,15,15,0,rigid);
	O.getScan(0,0, 0, 180, M_PI/180, occu, dist, s);
	O.setScanFillSimple(0, 0 ,0, s,empty,occu);
	draw(O,rgb,Nx,Ny);
	free(dist);
}

int main(void){
	TOccupancyGrid O(2.0f,2.0f,0.01f,Grid::TCoord2D(0,0));
	grid_t empty=0.0;
	grid_t rigid=254;
	grid_t occu=128;
	int Nx,Ny;
	unsigned char *rgb;
	unsigned char *gray;
	float *data;
	
	CJPEGEncoder encoder;
	
	///Initialize grid as empty
	O.setGridToValue(empty);
	
	O.getDim(Nx,Ny);
	rgb = (unsigned char *) malloc(3*Nx*Ny);
	
	///Initialize Image display
	addImageWindow(Nx, Ny);
	glutImageGUI_Start();
	
	gray = (unsigned char *)malloc(Nx*Ny);
	
	draw(O,rgb,Nx,Ny);
	data = O.getDataPtr();
	for(int i=0;i<O.size();i++){
		gray[i]=(unsigned char) 100*fabs(data[i]);
	}
	
	/*testPrimitives(O,rgb,Nx,Ny);
	testScan(O,rgb,Nx,Ny);
	testGetScan(O,rgb,Nx,Ny);
	testMCL(O,rgb,Nx,Ny);
	testDistanceTransform(O,rgb,Nx,Ny);*/
	
	testNormalDistribution(O,rgb,Nx,Ny);
	
	
		
	while(1){
		//O.fillRGB(rgb, empty, occu, rigid);		
		displayImage(0, rgb,Ny,Nx);
		usleep(500*1000);
	}
		
return 0;
}
